
#include <rtai_shm.h>
#include "rtai_shmem.h"

#ifndef __IN_RTAI__
#include <rtai_lxrt.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#endif


// 共享内存指针
SharedMemory *shm_ptr;

// 初始化共享内存
#ifdef __IN_RTAI__ 
void rtai_shmem_init(void) { 
    shm_ptr = rtai_kmalloc(SHM_KEY, sizeof(SharedMemory));
    if (!shm_ptr) {
        printk("Failed to allocate shared memory\n");
    }
}   
#else
void lxrt_shmem_init(void) {
    shm_ptr = rtai_malloc(SHM_KEY, sizeof(SharedMemory));
    if (!shm_ptr) {
        shm_ptr = rtai_malloc(SHM_KEY, 0);
        if (!shm_ptr) {
            printf("Failed to access shared memory\n");
            exit(1);
        }
    } 
}
#endif



// 字符串hash函数
static unsigned int hash(const char *str) {
    unsigned long hash = 5381;
    int c;
    while ((c = *str++))
        hash = ((hash << 5) + hash) ^ c;
    return hash % HASH_TABLE_SIZE;
}

// 查找变量索引
static int find_index(char *name) {
    int i;
    unsigned int h = hash(name);
    int index = shm_ptr->hash_table.entries[h].index;
    
    if (index != -1 && strcmp(shm_ptr->data[index].name, name) == 0) {
        return index;
    }
    
    for(i = 0; i < shm_ptr->index; i++){
        if(strcmp(shm_ptr->data[i].name, name) == 0){
            shm_ptr->hash_table.entries[h].index = i;
            strcpy(shm_ptr->hash_table.entries[h].name, name);
            return i;
        }
    }
    return -1;
}

int shmem_register(char *name, DataType type, void *value){
    unsigned int h = hash(name);
    if(find_index(name) != -1){
        return 0;
    }
    if(shm_ptr->index >= MAX_VAR_NUM){
        return -1;
    }
    
    strcpy(shm_ptr->data[shm_ptr->index].name, name);
    shm_ptr->data[shm_ptr->index].type = type;
    
    switch(type) {
        case TYPE_INT: shm_ptr->data[shm_ptr->index].value.int32 = *(int*)value; break;
        case TYPE_DINT: shm_ptr->data[shm_ptr->index].value.int64 = *(long*)value; break;
        case TYPE_FLOAT: shm_ptr->data[shm_ptr->index].value.float32 = *(float*)value; break;
        case TYPE_DOUBLE: shm_ptr->data[shm_ptr->index].value.double64 = *(double*)value; break;
        case TYPE_STRING: strcpy(shm_ptr->data[shm_ptr->index].value.str, (char*)value); break;
        case TYPE_UINT: shm_ptr->data[shm_ptr->index].value.uint32 = *(unsigned int*)value; break;
        case TYPE_UDINT: shm_ptr->data[shm_ptr->index].value.uint64 = *(unsigned long*)value; break;
        default: return -1;
    }
    
    shm_ptr->hash_table.entries[h].index = shm_ptr->index;
    strcpy(shm_ptr->hash_table.entries[h].name, name);
    
    shm_ptr->index++;
    return 0;
}

int shmem_unregister(char *name){
    int i;
    unsigned int h = hash(name);

    int index = find_index(name);
    if(index == -1) return -1;
    
    shm_ptr->hash_table.entries[h].index = -1;
    
    for(i = index; i < shm_ptr->index - 1; i++){
        shm_ptr->data[i] = shm_ptr->data[i+1];
    }
    
    // Clear the last item
    memset(&shm_ptr->data[shm_ptr->index - 1], 0, sizeof(Data));
    shm_ptr->index--;

    return 0;
}

int shmem_write(char *name, void *value){
    int index = find_index(name);
    if(index == -1) return -1;
    
    switch(shm_ptr->data[index].type) {
        case TYPE_INT: shm_ptr->data[index].value.int32 = *(int*)value; break;
        case TYPE_DINT: shm_ptr->data[index].value.int64 = *(long*)value; break;
        case TYPE_FLOAT: shm_ptr->data[index].value.float32 = *(float*)value; break;
        case TYPE_DOUBLE: shm_ptr->data[index].value.double64 = *(double*)value; break;
        case TYPE_STRING: strcpy(shm_ptr->data[index].value.str, (char*)value); break;
        case TYPE_UINT: shm_ptr->data[index].value.uint32 = *(unsigned int*)value; break;
        case TYPE_UDINT: shm_ptr->data[index].value.uint64 = *(unsigned long*)value; break;
        default: return -1;
    }
    return 0;
}

int shmem_read(char *name, void *value){
    int index = find_index(name);
    if(index == -1) return -1;
    
    switch(shm_ptr->data[index].type) {
        case TYPE_INT: *(int*)value = shm_ptr->data[index].value.int32; break;
        case TYPE_DINT: *(long*)value = shm_ptr->data[index].value.int64; break;
        case TYPE_FLOAT: *(float*)value = shm_ptr->data[index].value.float32; break;
        case TYPE_DOUBLE: *(double*)value = shm_ptr->data[index].value.double64; break;
        case TYPE_STRING: strcpy((char*)value, shm_ptr->data[index].value.str); break;
        case TYPE_UINT: *(unsigned int*)value = shm_ptr->data[index].value.uint32; break;
        case TYPE_UDINT: *(unsigned long*)value = shm_ptr->data[index].value.uint64; break;
        default: return -1;
    }
    return 0;
}
